package math.vector;

import math.Maths;

public class Vecmath 
{
    
    private Vecmath() 
    {
    }
    
    public static vec3f ORIGIN = new vec3f(0, 0, 0);
    public static vec3f AXIS_X = new vec3f(1, 0, 0);
    public static vec3f AXIS_Y = new vec3f(0, 1, 0);
    public static vec3f AXIS_Z = new vec3f(0, 0, 1);
    
    public static vec3f rotate(float x, float y, float z, float u, float v, float w, float angleRad) 
    {
        float p = u * Maths.sinRad(angleRad * 0.5f);
        float q = v * Maths.sinRad(angleRad * 0.5f);
        float r = w * Maths.sinRad(angleRad * 0.5f);
        float s = Maths.cosRad(angleRad * 0.5f);
        float[][] matRot = {
            { 1 - 2 * q * q - 2 * r * r, 2 * p * q - 2 * s * r, 2 * p * r + 2 * s * q },
            { 2 * p * q + 2 * s * r, 1 - 2 * p * p - 2 * r * r, 2 * q * r + 2 * s * p },
            { 2 * p * r - 2 * s * q, 2 * q * r - 2 * s * p, 1 - 2 * p * p - 2 * q * q }
        };
        float[][] pForRot = {{x}, {y}, {z}};
        float[][] result = Maths.multMatrix(matRot, pForRot);
        return new vec3f(result[0][0], result[1][0], result[2][0]);
    }
//        float[][] fullmatRot = {
//            {   1 - 2 * q * q - 2 * r * r,      2 * p * q - 2 * s * r,          2 * p * r + 2 * s * q       },
//            {   2 * p * q + 2 * s * r,          1 - 2 * p * p - 2 * r * r,      2 * q * r + 2 * s * p       },
//            {   2 * p * r - 2 * s * q,          2 * q * r - 2 * s * p,          1 - 2 * p * p - 2 * q * q   }
//        };
    
//        float[][] xmatRot = {
//            {   1,  0,              0                },
//            {   0,  1 - 2 * p * p,  2 * sx * p       },
//            {   0,  -2 * sx * p,    1 - 2 * p * p    }
//        };
//        float[][] ymatRot = {
//            {   1 - 2 * q * q,  0,  2 * sy * q      },
//            {   0,              1,  0               },
//            {   -2 * sy * q,    0,  1 - 2 * q * q   }
//        };
//        float[][] zmatRot = {
//            {   1 - 2 * r * r,  -2 * sz * r,    0   },
//            {   2 * sz * r,     1 - 2 * r * r,  0   },
//            {   0,              0,              1   }
//        };

    public static vec3f rotate(vec3f input, float u, float v, float w, float angleRad) 
    {
        return rotate(input.x, input.y, input.z, u, v, w, angleRad);
    }

    public static vec3f rotate(vec3f input, vec3f axis, float angleRad) 
    {
        return rotate(input.x, input.y, input.z, axis.x, axis.y, axis.z, angleRad);
    }

    public static vec3f rotateAboutYAxis(vec3f input, float angleRad) 
    {
        float q = Maths.sinRad(angleRad * 0.5f);
        float s = Maths.cosRad(angleRad * 0.5f);
        float[][] matRot = {
            { 1 - 2 * q * q, 0f, 2 * s * q },
            { 0f, 1f, 0f },
            { -2 * s * q, 0f, 1 - 2 * q * q }
        };
        float[][] pForRot = {{input.x}, {input.y}, {input.z}};
        float[][] result = Maths.multMatrix(matRot, pForRot);
        return new vec3f(result[0][0], result[1][0], result[2][0]);
    }

    public static float distanceBetween(vec3f u, vec3f v) 
    {
        double x2 = Math.pow(u.x - v.x, 2);
        double y2 = Math.pow(u.y - v.y, 2);
        double z2 = Math.pow(u.z - v.z, 2);
        return (float) Math.sqrt(x2 + y2 + z2);
    }

    public static float distanceSqBetween(vec3f u, vec3f v) 
    {
        double x2 = Math.pow(u.x - v.x, 2);
        double y2 = Math.pow(u.y - v.y, 2);
        double z2 = Math.pow(u.z - v.z, 2);
        return (float) (x2 + y2 + z2);
    }

    public static float dotProduct(vec3f u, vec3f v) 
    {
        return u.x * v.x + u.y * v.y + u.z * v.z;
    }

    public static float getAngleRadBetween(vec3f u, vec3f v) 
    {
        return Maths.acosRad(dotProduct(u, v) / (u.length() * v.length()));
    }

    public static float getAngleDegBetween(vec3f u, vec3f v) 
    {
        return Maths.acosDeg(dotProduct(u, v) / (u.length() * v.length()));
    }

    public static boolean bBoxContainsInclusive(float l, float r, float b, float t, float f, float n, vec3f vec) 
    {
        if (vec.x < l || vec.x > r) 
        {
            return false;
        }
        if (vec.y < b || vec.y > t) 
        {
            return false;
        }
        if (vec.z < f || vec.z > n) 
        {
            return false;
        }
        return true;
    }

    public static boolean bBoxContainsExclusive(float l, float r, float b, float t, float f, float n, vec3f vec) 
    {
        if (vec.x <= l || vec.x >= r) 
        {
            return false;
        }
        if (vec.y <= b || vec.y >= t) 
        {
            return false;
        }
        if (vec.z <= f || vec.z >= n) 
        {
            return false;
        }
        return true;
    }

    public static vec3f getInverted(vec3f vec) 
    {
        return new vec3f(-vec.x, -vec.y, -vec.z);
    }

    public static vec3f getSurfaceNormal(vec3f p, vec3f q, vec3f r) 
    {
        vec3f ab = subtract(p, q);
        vec3f ac = subtract(p, r);
        return crossProduct(ab, ac).getUnitVectorCopy();
    }

    public static vec3f crossProduct(vec3f u, vec3f v) 
    {
        float x = (u.y * v.z) - (u.z * v.y);
        float y = (u.z * v.x) - (u.x * v.z);
        float z = (u.x * v.y) - (u.y * v.x);
        return new vec3f(x, y, z);
    }

    public static vec3f scalarMult(float scalar, vec3f u) 
    {
        return new vec3f((scalar * u.x), (scalar * u.y), (scalar * u.z));
    }

    public static vec3f add(vec3f u, vec3f v) 
    {
        float x = u.x + v.x;
        float y = u.y + v.y;
        float z = u.z + v.z;
        return new vec3f(x, y, z);
    }

    public static vec3f add(vec3f u, vec3f v, vec3f w) 
    {
        float x = u.x + v.x + w.x;
        float y = u.y + v.y + w.y;
        float z = u.z + v.z + w.z;
        return new vec3f(x, y, z);
    }

    public static vec3f add(vec3f u, vec3f v, vec3f w, vec3f s) 
    {
        float x = u.x + v.x + w.x + s.x;
        float y = u.y + v.y + w.y + s.y;
        float z = u.z + v.z + w.z + s.z;
        return new vec3f(x, y, z);
    }

    public static vec3f subtract(vec3f u, vec3f v) 
    {
        float x = u.x - v.x;
        float y = u.y - v.y;
        float z = u.z - v.z;
        return new vec3f(x, y, z);
    }

    public static vec3f getProjectionOfOnto(vec3f u, vec3f v) 
    {
        float k = dotProduct(u, v) / v.lengthSq();
        return scalarMult(k, v);
    }

    public static vec3f sphericalInterp(vec3f u, vec3f v, float t) 
    {
        float angle = getAngleRadBetween(u, v);
        float tangle = t * angle;
        float rec_sinangle = 1.0f / Maths.sinRad(angle);
        float ku = (Maths.sinRad((1 - t) * angle) * rec_sinangle);
        float kv = Maths.sinRad(tangle) * rec_sinangle;
        return add(scalarMult(ku, u), scalarMult(kv, v));
    }

}
